
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mode_althold.c
  * @author     baiyang
  * @date       2021-9-3
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "mode.h"
#include "fms.h"
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
bool mode_althold_init(bool ignore_checks);
void mode_althold_run();
/*----------------------------------variable----------------------------------*/
ModeAltHold mode_althold;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
static ModeNumber mode_althold_mode_number() { return ALT_HOLD; }
static bool mode_althold_requires_GPS() { return false; }
static bool mode_althold_has_manual_throttle() { return false; }
static bool mode_althold_allows_arming(ArmingMethod method) { return true; };
static bool mode_althold_is_autopilot() { return false; }
static bool mode_althold_has_user_takeoff(bool must_navigate) { return !must_navigate;}
static bool mode_althold_allows_autotune() { return true; }
static bool mode_althold_allows_flip() { return true; }

static const char *mode_althold_name() { return "ALT_HOLD"; }
static const char *mode_althold_name4() { return "ALTH"; }

/*
 * Init and run calls for althold, flight mode
 */

Mode* get_mode_althold()
{
    return (Mode *)&mode_althold;
}

void mode_althold_ctor(Mode *mode)
{
    mode_althold.mode = *mode;

    mode_althold.mode.mode_number = mode_althold_mode_number;
    mode_althold.mode.requires_GPS = mode_althold_requires_GPS;
    mode_althold.mode.has_manual_throttle = mode_althold_has_manual_throttle;
    mode_althold.mode.allows_arming = mode_althold_allows_arming;
    mode_althold.mode.is_autopilot = mode_althold_is_autopilot;
    mode_althold.mode.has_user_takeoff = mode_althold_has_user_takeoff;
    mode_althold.mode.allows_autotune = mode_althold_allows_autotune;
    mode_althold.mode.allows_flip = mode_althold_allows_flip;
    mode_althold.mode.name = mode_althold_name;
    mode_althold.mode.name4 = mode_althold_name4;

    mode_althold.mode.init = mode_althold_init;
    mode_althold.mode.run = mode_althold_run;
}

// althold_init - initialise althold controller
bool mode_althold_init(bool ignore_checks)
{

    // initialise the vertical position controller
    if (!posctrl_is_active_z(fms.pos_control)) {
        posctrl_init_z_controller(fms.pos_control);
    }

    // set vertical speed and acceleration limits
    posctrl_set_max_speed_accel_z(fms.pos_control, -fms_get_pilot_speed_dn(), fms_get_pilot_speed_up(), fms_get_pilot_accel_z());
    posctrl_set_correction_speed_accel_z(fms.pos_control, -fms_get_pilot_speed_dn(), fms_get_pilot_speed_up(), fms_get_pilot_accel_z());

    return true;
}

// althold_run - runs the althold controller
// should be called at 100hz or more
void mode_althold_run()
{
    // set vertical speed and acceleration limits
    posctrl_set_max_speed_accel_z(fms.pos_control, -fms_get_pilot_speed_dn(), fms_get_pilot_speed_up(), fms_get_pilot_accel_z());

    // apply SIMPLE mode transform to pilot inputs
    //update_simple_mode();

    // get pilot desired lean angles
    float target_roll, target_pitch;
    fms_get_pilot_desired_lean_angles(&target_roll, &target_pitch, PARAM_GET_INT16(VEHICLE,ANGLE_MAX), attctrl_get_althold_lean_angle_max(fms.attitude_control));

    // get pilot's desired yaw rate
    float target_yaw_rate = fms_get_pilot_desired_yaw_rate(fms.channel_yaw->control_in);

    // get pilot desired climb rate
    float target_climb_rate = fms_get_pilot_desired_climb_rate(fms.channel_throttle->control_in);
    target_climb_rate = math_constrain_float(target_climb_rate, -fms_get_pilot_speed_dn(), fms_get_pilot_speed_up());

    // Alt Hold State Machine Determination
    AltHoldModeState althold_state = mode_get_alt_hold_state(target_climb_rate);

    // Alt Hold State Machine
    switch (althold_state) {

    case AltHold_MotorStopped:
        attctrl_reset_yaw_target_and_rate(fms.attitude_control, false);
        attctrl_reset_rate_controller_I_terms(fms.attitude_control);
        posctrl_relax_z_controller(fms.pos_control, 0.0f);   // forces throttle output to decay to zero
        break;

    case AltHold_Landed_Ground_Idle:
        attctrl_reset_yaw_target_and_rate(fms.attitude_control, true);
    case AltHold_Landed_Pre_Takeoff:
        attctrl_reset_rate_controller_I_terms_smoothly(fms.attitude_control);
        posctrl_relax_z_controller(fms.pos_control, 0.0f);   // forces throttle output to decay to zero
        break;

    case AltHold_Takeoff:
        // initiate take-off
        if (!mode_althold.mode.takeoff->_running) {
            takeoff_start(mode_althold.mode.takeoff, math_constrain_float(PARAM_GET_FLOAT(VEHICLE,PILOT_TKOFF_ALT),0.0f,1000.0f));
        }

        // get avoidance adjusted climb rate
        //target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);

        // set position controller targets adjusted for pilot input
        takeoff_do_pilot_takeoff(mode_althold.mode.takeoff, target_climb_rate);
        break;

    case AltHold_Flying:
        Motors_set_desired_spool_state(fms.motors, MOTOR_DESIRED_THROTTLE_UNLIMITED);

//#if AC_AVOID_ENABLED == ENABLED
        // apply avoidance
        //copter.avoid.adjust_roll_pitch(target_roll, target_pitch, copter.aparm.angle_max);
//#endif

        // adjust climb rate using rangefinder
        //target_climb_rate = copter.surface_tracking.adjust_climb_rate(target_climb_rate);

        // get avoidance adjusted climb rate
        //target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);

        posctrl_set_pos_target_z_from_climb_rate_cm(fms.pos_control, target_climb_rate, false);
        break;
    }

    // call attitude controller
    attctrl_input_euler_angle_roll_pitch_euler_rate_yaw(fms.attitude_control,target_roll, target_pitch, target_yaw_rate);
    
    // run the vertical position controller and set output throttle
    posctrl_update_z_controller(fms.pos_control);
}

/*------------------------------------test------------------------------------*/


